#include "../../include/driver/driver_usart.h"
#include "../../include/device/imuDevice.h"
#include <iostream> 
#include<vector>
#include<iostream>
#include<algorithm> // 头文件

C_imuDevice::C_imuDevice(const char *_path, int _speed)
{
    spdlog::info("C_rtkDevice init {} {} \r\n", _path, _speed);
    m_serial = NULL;
    switch (_speed)  {
    case 460800:
        m_serial = new SerialDriver(_path, BPS460800); // 串口驱动对象，包含了对串口的初始化
        if(m_serial->SerialDriver_state != SERIALDRIVER_OPEN){
            spdlog::error("error! C_rtkDevice:: SerialDriver wrong");
        }
        break;
    case 115200:
        m_serial = new SerialDriver(_path, BPS115200); // 串口驱动对象，包含了对串口的初始化
        if(m_serial->SerialDriver_state != SERIALDRIVER_OPEN){
            spdlog::error("error! C_rtkDevice:: SerialDriver wrong");
        }
        break;
    default:
        spdlog::error("error! C_imuDevice:: SerialDriver band input wrong");
        break;
    }
    m_recvBuf = (uint8_t*)malloc(512*(sizeof(uint8_t)));
}

C_imuDevice::~C_imuDevice()
{
    delete m_serial; m_serial = NULL;
}

int32_t  findData(std::vector<uint8_t> &dataBuf, int16_t data, int16_t startLocate)
{
    for(int i=startLocate; i<dataBuf.size(); i++){
        if(data = dataBuf[i]){
            return i;
        }
    }
    return -1;
}

int32_t C_imuDevice::getUsartData(std::vector<uint8_t> &dataBuf)
{
    /*数据帧寻找方法：
    程序开始有空的固定数据缓冲器buf，如果有接收到数据就将数据储插入到固定的缓冲区buf的最后面的位置。
    到数据帧头和帧尾的位置，将完整的数据帧保存到函数输出缓冲器中，作为函数的本次输出结果。 
    将这次找到的数据帧尾到结束的位置到这一次接收的数据的最后位置的这些剩余所有数据都转存到临时buf中，
    再将固定缓冲区中的buf内容替换成临时buf的内容，等待下一次数据*/ 
    std::vector<uint8_t> data_buffer;
    std::vector<uint8_t> tempBuf;
    std::vector<uint8_t> reamin_buffer; // 存放剩余数据
    uint32_t headLocate = 0;
    uint32_t endLocate = 0;
    uint32_t payloadNum = 0;

    /* 接受串口的数据*/
    int32_t read_result = m_serial->ReadBytes(data_buffer, 0, 10);
    if(read_result == RECV_DONE){
        cache_buffer.insert(cache_buffer.end(), data_buffer.begin(), data_buffer.end());
    } else if(read_result == REV_ERROR){
        return REV_ERROR;
    }
    
    /* 寻找cache buffer内的数据包的位置,根据包头和包尾确定整体的位置 */
    if(cache_buffer.size()>0){ // 由于接受函数从程序开始一直会执行，所以需要通过判断是否有数据决定是否返回数据
        headLocate = findData(cache_buffer, DATA_BEGIN, 0);
        if(headLocate != -1){
            endLocate = findData(cache_buffer, DATA_END, headLocate);
            if(endLocate == -1){
                return REV_NULL;
            }
        } else {
            return REV_NULL;
        }
        payloadNum = cache_buffer[headLocate+2]; // 数据段的长度
        if(headLocate+payloadNum+7 < cache_buffer.size()){ // 判断接收到的数据长度是不是小于数据包的长度，如果小于的话说明数据还没有接收完成
            //spdlog::info("headLocate:{} payloadNum:{} cache_buffer last data: {} ", headLocate, payloadNum, cache_buffer[headLocate+payloadNum+7]);
            if(cache_buffer[headLocate+payloadNum+7] == DATA_END){ // 如果固定的位置没有数据结束位说明数据是有问题的
                for(int i=0; i<=payloadNum+7; i++){ // 把整个数据包的数据储存到临时buf中
                tempBuf.insert(tempBuf.end(), cache_buffer[headLocate+i]);
                //printf("%02x ", cache_buffer[headLocate+i]);
                } //printf("\r\n\r\n");
                reamin_buffer.insert(reamin_buffer.end(), cache_buffer.begin()+headLocate+payloadNum+8, cache_buffer.end()); //转存剩余的数据
                cache_buffer = reamin_buffer; // 更新数据包的数据
                dataBuf = tempBuf; // 输出最终数据
                return RECV_DONE;
            } else {
                spdlog::error("C_imuDevice::getUsartData data error!! data end is wrong");
                for(int i=0; i<cache_buffer.size(); i++){
                    printf("%02x ", cache_buffer[i]);
                } printf("\r\n");
                cache_buffer = reamin_buffer; // 更新储存内存
                return REV_NULL;
            }
        }
    }
    return REV_NULL;
}